#include "TesRay.h"
#include"TesPlane.h"

#ifndef IS_ZERO 
#define ZERO(X) ( X < 0.000001f && X > -0.000001f)
#define IS_ZERO ZERO
#endif

TesRay::TesRay() {
	t = 1.f;
	Org.Zero();
	Dir.Zero();
}

TesRay::TesRay(const TesVector3&  org, const TesVector3& dir) {
	t = 1.f;
	Org = org;
	Dir = dir;
}

TesVector3 TesRay::getNearestPoint(TesVector3& Point) {
	TesVector3 temp = (Org + Dir).Projection(Point - Org);
	return Org + temp;
}

bool TesRay::Collide(TesRay& Other, TesVector3* hitPos) {
	float tTemp = .0f;
	tTemp = (((Other.Org - Org).CrossProduct(Other.Dir)) * (Dir.CrossProduct(Other.Dir))) / ((Dir.CrossProduct(Other.Dir)).SqrLength());
	if (tTemp < .0f && tTemp > 1.f)
		return false;
	if (hitPos)
		*hitPos = Org + tTemp * Dir;
	return true;
}

bool TesRay::checkPtIn(TesVector3& Point) {
	TesVector3 temp = Point - Org;
	const float i = 0.000001f;
	float NumTemp = temp * Dir;
	if (!(NumTemp < 1.f + i || NumTemp > 1.f - i))
		return false;
	return !(temp.Length() > Dir.Length());
}

bool TesRay::Collide(TesPlane& Plane, TesVector3* hitPos) {
	float tTemp = .0f;
	tTemp = (Plane.d - (Org * Plane.Normal)) / (Dir * Plane.Normal);
	if (tTemp < .0f && tTemp > 1.f)
		return false;
	if (hitPos)
		*hitPos = Org + tTemp * Dir;
	return true;
}